
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       srv_channel_aux.c
  * @author     baiyang
  * @date       2021-12-30
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "srv_channel.h"

#include <string.h>

#include <rtthread.h>

#include <rc_channel/rc_channel.h>
#include <common/console/console.h>
#include <common/gp_math/gp_mathlib.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/*
  return the current function for a channel
*/
Aux_servo_function_t srv_channels_channel_function(uint8_t channel)
{
    if (channel < NUM_SERVO_CHANNELS) {
        return (Aux_servo_function_t)srvs.channels[channel].function;
    }
    return k_none;
}

/*
  call output_ch() on all channels
 */
void srv_channels_output_ch_all(void)
{
    for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
        srv_channel_output_ch(&srvs.channels[i]);
    }
}

// set output pwm to trim for the given function
void srv_channels_set_output_to_trim(Aux_servo_function_t function)
{
    for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
        if (srvs.channels[i].function == function) {
            srv_channel_set_output_pwm(&srvs.channels[i], srvs.channels[i].servo_trim, false);
        }
    }
}

/// setup the output range types of all functions
void srv_channels_update_aux_servo_function(void)
{
    if (!srvs.channels) {
        return;
    }

    memset(srvs.function_mask, 0, sizeof(srvs.function_mask));

    for (uint16_t i = 0; i < k_nr_aux_servo_functions; i++) {
        srvs.functions[i].channel_mask = 0;
    }

    // set auxiliary ranges
    for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
        if (!srv_channel_valid_function(&srvs.channels[i])) {
            continue;
        }
        const uint16_t function = (uint16_t)srvs.channels[i].function;
        srv_channel_aux_servo_function_setup(&srvs.channels[i]);

        uint16_t word = function/32;
        uint8_t ofs = function & 0x1f;
        srvs.function_mask[word] |= (1U << ofs);

        srvs.functions[function].channel_mask |= 1U<<i;
    }
    srvs.initialised = true;
}

/// Should be called after the the servo functions have been initialized
/// called at 1Hz
void srv_channels_enable_aux_servos()
{
    srv_hal_set_default_rate((uint16_t)srvs.default_rate);

    srv_channels_update_aux_servo_function();

    // enable all channels that are set to a valid function. This
    // includes k_none servos, which allows those to get their initial
    // trim value on startup
    for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
        srv_channel *c = &srvs.channels[i];
        // see if it is a valid function
        if (srv_channel_valid_function(c)) {
            srv_hal_enable_ch(c->ch_num);
        } else {
            srv_hal_disable_ch(c->ch_num);
        }

        // output some servo functions before we fiddle with the
        // parameter values:
        if ((Aux_servo_function_t)c->function == k_min) {
            srv_channel_set_output_pwm(c, c->servo_min, false);
            srv_channel_output_ch(c);
        } else if ((Aux_servo_function_t)c->function == k_trim) {
            srv_channel_set_output_pwm(c, c->servo_trim, false);
            srv_channel_output_ch(c);
        } else if ((Aux_servo_function_t)c->function == k_max) {
            srv_channel_set_output_pwm(c, c->servo_max, false);
            srv_channel_output_ch(c);
        }
    }

    // propagate channel masks to the ESCS
    srv_hal_update_channel_masks();
}

/*
    for channels which have been marked as digital output then the
    MIN/MAX/TRIM values have no meaning for controlling output, as
    the HAL handles the scaling. We still need to cope with places
    in the code that may try to set a PWM value however, so to
    ensure consistency we force the MIN/MAX/TRIM to be consistent
    across all digital channels. We use a MIN/MAX of 1000/2000, and
    set TRIM to either 1000 or 1500 depending on whether the channel
    is reversible
*/
void srv_channels_set_digital_outputs(uint16_t dig_mask, uint16_t rev_mask)
{
    srvs.digital_mask |= dig_mask;
    srvs.reversible_mask |= rev_mask;

    for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
        srv_channel *c = &srvs.channels[i];
        if (srvs.digital_mask & (1U<<i)) {
            c->servo_min = 1000;
            c->servo_max = 2000;
            if (srvs.reversible_mask & (1U<<i)) {
                c->servo_trim = 1500;
            } else {
                c->servo_trim = 1000;
            }
        }
    }
}

/// enable output channels using a channel mask
void srv_channels_enable_by_mask(uint16_t mask)
{
    for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
        if (mask & (1U<<i)) {
            srv_hal_enable_ch(i);
        }
    }
}

/*
  set radio_out for all channels matching the given function type
 */
void srv_channels_set_output_pwm(Aux_servo_function_t function, uint16_t value)
{
    if (!srv_channels_function_assigned(function)) {
        return;
    }
    for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
        if ((Aux_servo_function_t)srvs.channels[i].function == function) {
            srv_channel_set_output_pwm(&srvs.channels[i], value, false);
            srv_channel_output_ch(&srvs.channels[i]);
        }
    }
}

/*
  return true if a particular function is assigned to at least one RC channel
 */
bool srv_channels_function_assigned(Aux_servo_function_t function)
{
    if (!srvs.initialised) {
        srv_channels_update_aux_servo_function();
    }

    uint16_t word = (uint16_t)function/32;
    uint8_t ofs = function & 0x1f;

    return (srvs.function_mask[word] & (1U << ofs)) != 0;
}

/*
  set radio_out for all channels matching the given function type
  trim the output assuming a 1500 center on the given value
  reverses pwm output based on channel reversed property
 */
void srv_channels_set_output_pwm_trimmed(Aux_servo_function_t function, int16_t value)
{
    if (!srv_channels_function_assigned(function)) {
        return;
    }
    for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
        if ((Aux_servo_function_t)srvs.channels[i].function == function) {
            int16_t value2;
            if (srvs.channels[i].reversed) {
                value2 = 1500 - value + srvs.channels[i].servo_trim;
            } else {
                value2 = value - 1500 + srvs.channels[i].servo_trim;
            }
            srv_channel_set_output_pwm(&srvs.channels[i], math_constrain_int16(value2,srvs.channels[i].servo_min,srvs.channels[i].servo_max), false);
            srv_channel_output_ch(&srvs.channels[i]);
          }
    }
}

/*
  set and save the trim value to current output for all channels matching
  the given function type
 */
void srv_channels_set_trim_to_servo_out_for(Aux_servo_function_t function)
{
    if (!srv_channels_function_assigned(function)) {
        return;
    }
    for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
        if ((Aux_servo_function_t)srvs.channels[i].function == function) {
            //channels[i].servo_trim.set_and_save_ifchanged(channels[i].get_output_pwm());
            srvs.channels[i].servo_trim = srvs.channels[i].output_pwm;
        }
    }
}

/*
  setup failsafe value for an auxiliary function type to a Limit
 */
void srv_channels_set_failsafe_pwm(Aux_servo_function_t function, uint16_t pwm)
{
    if (!srv_channels_function_assigned(function)) {
        return;
    }
    for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
        const srv_channel *c = &srvs.channels[i];
        if ((Aux_servo_function_t)c->function == function) {
            srv_hal_set_failsafe_pwm(1U<<c->ch_num, pwm);
        }
    }
}

/*
  setup failsafe value for an auxiliary function type to a Limit
 */
void srv_channels_set_failsafe_limit(Aux_servo_function_t function, enum SRVLimit limit)
{
    if (!srv_channels_function_assigned(function)) {
        return;
    }
    for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
        srv_channel *c = &srvs.channels[i];
        if ((Aux_servo_function_t)c->function == function) {
            uint16_t pwm = srv_channel_get_limit_pwm(c, limit);
            srv_hal_set_failsafe_pwm(1U<<c->ch_num, pwm);
        }
    }
}

/*
  set radio output value for an auxiliary function type to a Limit
 */
void srv_channels_set_output_limit(Aux_servo_function_t function, enum SRVLimit limit)
{
    if (!srv_channels_function_assigned(function)) {
        return;
    }
    for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
        srv_channel *c = &srvs.channels[i];
        if ((Aux_servo_function_t)c->function == function) {
            uint16_t pwm = srv_channel_get_limit_pwm(c, limit);
            srv_channel_set_output_pwm(c, pwm, false);
            if ((Aux_servo_function_t)c->function == k_manual) {
                rc_channel_t cin = rcs_chan(c->ch_num);
                if (cin != NULL) {
                    // in order for output_ch() to work for k_manual we
                    // also have to override radio_in
                   cin->radio_in = pwm;
                }
            }
        }
    }
}

/*
  set the default channel an auxiliary output function should be on
 */
bool srv_channels_set_aux_channel_default(Aux_servo_function_t function, uint8_t channel)
{
    if (srv_channels_function_assigned(function)) {
        // already assigned
        return true;
    }
    if ((Aux_servo_function_t)srvs.channels[channel].function != k_none) {
        if ((Aux_servo_function_t)srvs.channels[channel].function == function) {
            return true;
        }
        console_printf("Channel %u already assigned function %u\n",
                            (unsigned)(channel + 1),
                            (unsigned)srvs.channels[channel].function);
        return false;
    }
    srvs.channels[channel].type_setup = false;
    srvs.channels[channel].function = function;
    srv_channel_aux_servo_function_setup(&srvs.channels[channel]);

    // ignore an invalid bit number
    if (function < k_nr_aux_servo_functions) {
        uint16_t word = (uint16_t)function/32;
        uint8_t ofs = (uint16_t)function & 0x1f;
        srvs.function_mask[word] |= (1U << ofs);
    }

    if (srv_channel_valid_function2(function)) {
        srvs.functions[function].channel_mask |= 1U<<channel;
    }
    return true;
}

// find first channel that a function is assigned to
bool srv_channels_find_channel(Aux_servo_function_t function, uint8_t *chan)
{
    if (!srv_channels_function_assigned(function)) {
        return false;
    }
    for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
        if ((Aux_servo_function_t)srvs.channels[i].function == function) {
            *chan = srvs.channels[i].ch_num;
            return true;
        }
    }
    return false;
}

/*
  get a pointer to first auxillary channel for a channel function
*/
srv_channel *srv_channels_get_channel_for(Aux_servo_function_t function, int8_t default_chan)
{
    uint8_t chan;
    if (default_chan >= 0) {
        srv_channels_set_aux_channel_default(function, default_chan);
    }
    if (!srv_channels_find_channel(function, &chan)) {
        return NULL;
    }
    return &srvs.channels[chan];
}

void srv_channels_set_output_scaled(Aux_servo_function_t function, float value)
{
    if (srv_channel_valid_function2(function)) {
        srvs.functions[function].output_scaled = value;
        srv_have_pwm_mask &= ~srvs.functions[function].channel_mask;
    }
}

float srv_channels_get_output_scaled(Aux_servo_function_t function)
{
    if (srv_channel_valid_function2(function)) {
        return srvs.functions[function].output_scaled;
    }
    return 0;
}

// get slew limited scaled output for the given function type
float srv_channels_get_slew_limited_output_scaled(Aux_servo_function_t function)
{
    if (!srv_channel_valid_function2(function)) {
        return 0.0f;
    }
    for (struct slew_list *slew = srvs._slew; slew; slew = slew->next) {
        if (slew->func == function) {
            if (!math_flt_positive(slew->max_change)) {
                // treat negative or zero slew rate as disabled
                break;
            }
            return math_constrain_float(srvs.functions[function].output_scaled, slew->last_scaled_output - slew->max_change, slew->last_scaled_output + slew->max_change);
        }
    }
    // no slew limiting
    return srvs.functions[function].output_scaled;
}

/*
  get mask of output channels for a function
 */
uint16_t srv_channels_get_output_channel_mask(Aux_servo_function_t function)
{
    if (!srvs.initialised) {
        srv_channels_update_aux_servo_function();
    }
    if (srv_channel_valid_function2(function)) {
        return srvs.functions[function].channel_mask;
    }
    return 0;
}

// set the trim for a function channel to given pwm
void srv_channels_set_trim_to_pwm_for(Aux_servo_function_t function, int16_t pwm)
{
    for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
        if ((Aux_servo_function_t)srvs.channels[i].function == function) {
            srvs.channels[i].servo_trim = pwm;
        }
    }
}

// set the trim for a function channel to min output of the channel honnoring reverse unless ignore_reversed is true
void srv_channels_set_trim_to_min_for(Aux_servo_function_t function, bool ignore_reversed)
{
    for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
        if ((Aux_servo_function_t)srvs.channels[i].function == function) {
            srvs.channels[i].servo_trim = ((srvs.channels[i].reversed && !ignore_reversed)?srvs.channels[i].servo_max:srvs.channels[i].servo_min);
        }
    }
}

// get pwm output for the first channel of the given function type.
bool srv_channels_get_output_pwm(Aux_servo_function_t function, uint16_t *value)
{
    uint8_t chan;
    if (!srv_channels_find_channel(function, &chan)) {
        return false;
    }
    if (!srv_channel_valid_function2(function)) {
        return false;
    }
    srv_channel_calc_pwm(&srvs.channels[chan], srvs.functions[function].output_scaled);
    *value = srvs.channels[chan].output_pwm;
    return true;
}

// call set_angle() on matching channels
void srv_channels_set_angle(Aux_servo_function_t function, uint16_t angle)
{
    for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
        if ((Aux_servo_function_t)srvs.channels[i].function == function) {
            srv_channel_set_angle(&srvs.channels[i], angle);
        }
    }    
}

// call set_range() on matching channels
void srv_channels_set_range(Aux_servo_function_t function, uint16_t range)
{
    for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
        if ((Aux_servo_function_t)srvs.channels[i].function == function) {
            srv_channel_set_range(&srvs.channels[i], range);
        }
    }
}

// set MIN parameter for a function
void srv_channels_set_output_min_max(Aux_servo_function_t function, uint16_t min_pwm, uint16_t max_pwm)
{
    for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
        if ((Aux_servo_function_t)srvs.channels[i].function == function) {
            srv_channel_set_output_min(&srvs.channels[i], min_pwm);
            srv_channel_set_output_max(&srvs.channels[i], max_pwm);
        }
    }
}

// set RC output frequency on a function output
void srv_channels_set_rc_frequency(Aux_servo_function_t function, uint16_t frequency_hz)
{
    uint16_t mask = 0;
    for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
        srv_channel *c = &srvs.channels[i];
        if ((Aux_servo_function_t)c->function == function) {
            mask |= (1U<<c->ch_num);
        }
    }
    if (mask != 0) {
        srv_hal_set_freq(mask, frequency_hz);
    }
}

// return true if all of the outputs in mask are digital
bool srv_channels_have_digital_outputs(uint16_t mask)
{
    return mask != 0 && (mask & srvs.digital_mask) == mask;
}

// setup output ESC scaling based on a channels MIN/MAX
void srv_channels_set_esc_scaling_for(Aux_servo_function_t function)
{
    uint8_t chan;
    if (srv_channels_find_channel(function, &chan)) {
        srv_hal_set_esc_scaling(srv_channel_get_output_min(&srvs.channels[chan]), srv_channel_get_output_max(&srvs.channels[chan]));
    }
}

/*
  limit slew rate for an output function to given rate in percent per
  second. This assumes output has not yet done to the hal
 */
void srv_channels_set_slew_rate(Aux_servo_function_t function, float slew_rate, uint16_t range, float dt)
{
    if (!srv_channel_valid_function2(function)) {
        return;
    }
    const float max_change = range * slew_rate * 0.01 * dt;

    for (struct slew_list *slew = srvs._slew; slew; slew = slew->next) {
        if (slew->func == function) {
            // found existing item, update max change
            slew->max_change = max_change;
            return;
        }
    }

    if (!math_flt_positive(max_change)) {
        // no point in adding a disabled slew limit
        return;
    }

    // add new item
    struct slew_list *new_slew = (struct slew_list *)rt_malloc(sizeof(struct slew_list));
    if (new_slew == NULL) {
        return;
    }
    new_slew->func = function;
    new_slew->last_scaled_output = srvs.functions[function].output_scaled;
    new_slew->max_change = max_change;
    new_slew->next = srvs._slew;
    srvs._slew = new_slew;
}

/*
  copy radio_in to radio_out for a given function
 */
void srv_channels_copy_radio_in_out(Aux_servo_function_t function, bool do_input_output)
{
    if (!srv_channels_function_assigned(function)) {
        return;
    }
    for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
        if (srvs.channels[i].function == function) {
            rc_channel_t c = rcs_chan(srvs.channels[i].ch_num);
            if (c == NULL) {
                continue;
            }
            srv_channel_set_output_pwm(&srvs.channels[i], rc_get_radio_in(c), false);
            if (do_input_output) {
                srv_channel_output_ch(&srvs.channels[i]);
            }
        }
    }
}

/*
  copy radio_in to radio_out for a channel mask
 */
void srv_channels_copy_radio_in_out_mask(uint32_t mask)
{
    for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
        if ((1U<<i) & mask) {
            rc_channel_t c = rcs_chan(srvs.channels[i].ch_num);
            if (c == NULL) {
                continue;
            }
            srv_channel_set_output_pwm(&srvs.channels[i], rc_get_radio_in(c), false);
        }
    }

}

// return true when auto_trim enabled
bool srv_channels_auto_trim_enabled(void)
{
    return srvs.auto_trim;
}

/*
  auto-adjust channel trim from an integrator value. Positive v means
  adjust trim up. Negative means decrease
 */
void srv_channels_adjust_trim(Aux_servo_function_t function, float v)
{
    if (math_flt_zero(v)) {
        return;
    }
    for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
        srv_channel *c = &srvs.channels[i];
        if (function != c->function) {
            continue;
        }
        float change = c->reversed?-v:v;
        uint16_t new_trim = c->servo_trim;
        if (c->servo_max <= c->servo_min) {
            continue;
        }
        float trim_scaled = (float)(c->servo_trim - c->servo_min) / (c->servo_max - c->servo_min);
        if (change > 0 && trim_scaled < 0.6f) {
            new_trim++;
        } else if (change < 0 && trim_scaled > 0.4f) {
            new_trim--;
        } else {
            continue;
        }
        c->servo_trim = new_trim;

        srvs.trimmed_mask |= 1U<<i;
    }
}
/*------------------------------------test------------------------------------*/


